


#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include <QString>
#include <QJsonDocument>
#include <QJsonObject>


namespace rviz_custom_plugins
{

namespace utils
{


QJsonObject getMetadata(InteractiveMarker& intMarker)
{
    // 获取intMarker.controls中name="metadata"的control
    // 返回control.description转换的json对象
    for (auto& control : intMarker.controls) {
        if (control.name == "metadata") 
        {
            QString metadata = QString::fromStdString(control.description);
            QJsonDocument metaDoc = QJsonDocument::fromJson(metadata.toUtf8());
            QJsonObject metaJsonObj = metaDoc.object();
            return metaJsonObj;
        }
    }

    // 如果不存在的话则新建一个空的control
    // 并把control放入intMarker.controls中
    QJsonObject metaJsonObj;
    QJsonDocument metaDoc(metaJsonObj);
    QString jsonStr = metaDoc.toJson(QJsonDocument::Compact);
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.name = "metadata";
    control.always_visible = false;
    control.description = jsonStr.toStdString();
    intMarker.controls.push_back(control);
    return metaJsonObj;
}


QString getMetadataItem(InteractiveMarker& intMarker, const QString & key, const QString & defaultValue)
{
    QJsonObject metaJsonObj = getMetadata(intMarker);
    return metaJsonObj.contains(key) ? metaJsonObj[key].toString() : defaultValue;
}


void setMetadata(InteractiveMarker& intMarker, const QJsonObject& metaJsonObj)
{ 
    // remove掉intMarker.controls中name="metadata"的control
    for (auto it = intMarker.controls.begin(); it != intMarker.controls.end(); ) {
        if (it->name == "metadata") {
            it = intMarker.controls.erase(it);  // 删除并更新迭代器
        } else {
            ++it;  // 正常前进
        }
    }

    QJsonDocument metaDoc(metaJsonObj);
    QString jsonStr = metaDoc.toJson(QJsonDocument::Compact);
    visualization_msgs::msg::InteractiveMarkerControl control;
    control.name = "metadata";
    control.always_visible = false;
    control.description = jsonStr.toStdString();
    intMarker.controls.push_back(control);
}


void addMetadata(InteractiveMarker& intMarker, const QString & key, const QString & value)
{
    QJsonObject metaJsonObj = getMetadata(intMarker);
    metaJsonObj[key] = value;
    setMetadata(intMarker, metaJsonObj);
}


}

}
